
#include <ros/ros.h>
#include "fibonacci_action/fibonacciAction.h"
#include <actionlib/server/simple_action_server.h>

typedef actionlib::SimpleActionServer<fibonacci_action::fibonacciAction> Server;

void execute(const fibonacci_action::fibonacciGoalConstPtr &goal, Server *server)
{
    int num =  goal->order;
    fibonacci_action::fibonacciFeedback feedback;//连续反馈
    ros::Rate rate(10);//通过频率设置休眠时间
    std::vector<int> fibon_vec;

    int a =0, b =0, c =1;
    ROS_INFO("=========fibonacci excute=========");
    fibon_vec.push_back(c);
    feedback.sequence = fibon_vec;
    server->publishFeedback(feedback);
    rate.sleep();
    for(int i =1; i <= goal->order; i++)
    {
        a = b;
        b = c;
        c = a + b;

        fibon_vec.push_back(c);
        feedback.sequence = fibon_vec;
        server->publishFeedback(feedback);
        rate.sleep();
    }
    
    //设置最终结果
    fibonacci_action::fibonacciResult r;
    r.sequence = fibon_vec;
    server->setSucceeded(r);
    ROS_INFO("=========fibonacci end=========");
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "fibonacci_server");
    ros::NodeHandle n;
    ROS_INFO("=========fibonacci server start=========");
    Server server(n, "/fibonacci_action", boost::bind(&execute, _1, &server), false);
    server.start();
    ros::spin();
    return 0;
}